'''
Description: 
Author: 
LastEditors: Please set LastEditors
Date: 2023-03-09 14:48:19
LastEditTime: 2023-03-20 14:13:31

#原理
先取x最小点为起点（x最小必然是凸点）
以起点为原点变换坐标
叉乘：矢量P（X1，Y1）,矢量Q（X2，Y2）,则PxQ=X1*Y2-X2*Y1

若PxQ>0，则P在Q的顺时针方向

若PxQ<0，则P在Q的逆时针方向

若PxQ>0，则P和Q共线，但可能同向也可能反向

'''
# -*- coding:utf-8 -*-

from loguru import logger
import numpy as np
import math


class PolygonIOU:
    def __init__(self):
        super().__init__()

    def __cross_product(self, x1, y1, x2, y2):
        return x1*y2-x2*y1

    def sort_points(self, point_array: list) -> list:
        """
        对输入点排序[[x1,y1],[x2,y2]....]
        """
        result = []
        if len(point_array) < 3:
            logger.error('no points need sort')
            return point_array

        if len(point_array) > 10000:
            logger.error('too many data')
            return point_array

        # 查找x最小
        x_min = point_array[0][0]
        i_min = 0
        for i in range(len(point_array)):
            if point_array[i][0] < x_min:
                x_min = point_array[i][0]
                i_min = i
        # 开始点
        x0 = point_array[i_min][0]
        y0 = point_array[i_min][1]
        result.append([x0, y0])

        # 排序
        for i in range(len(point_array)):
            if i == i_min:  # 跳过原点
                continue
            x1 = point_array[i][0]-x0
            y1 = point_array[i][1]-y0

            for j in range(i+1, len(point_array)):
                if j == i_min:  # 跳过原点
                    continue
                x2 = point_array[j][0]-x0
                y2 = point_array[j][1]-y0
            #######################################################################################
                PxQ = self.__cross_product(x1, y1, x2, y2)
                # 两点对调。判断标准，逆时针，如果同轴，判断距离原点更远
                if PxQ > 0 or (0 == PxQ and (abs(x1)+abs(y1) > abs(x2)+abs(y2))):
                    # 对调
                    xtemp = point_array[j][0]
                    ytemp = point_array[j][1]

                    point_array[j][0] = point_array[i][0]
                    point_array[j][1] = point_array[i][1]

                    point_array[i][0] = xtemp
                    point_array[i][1] = ytemp

                    x1 = x2
                    y1 = y2

            result.append([point_array[i][0], point_array[i][1]])
            ##########################################################################################
        return result

    def is_convex_polygon(self, point_array: list) -> bool:
        '''
        判断是否是凸多边形
        要先排序        
        '''
        if len(point_array) < 4:
            return True

        for i in range(len(point_array)-1):  # 排序后0点一定是凸点，不用判断
            x1 = point_array[(i+1)][0]-point_array[i][0]
            y1 = point_array[(i+1)][1]-point_array[i][1]
            x2 = point_array[(i+2) % len(point_array)][0]-point_array[i+1][0]
            y2 = point_array[(i+2) % len(point_array)][1]-point_array[i+1][1]
            PxQ = self.__cross_product(x1, y1, x2, y2)
            if PxQ > 0:
                return False
        return True

    def convex_to_converx(self, point_array: list) -> list:
        '''
        将凹多边形，转换成凸多边形。凹的点会被移除
        要先排序        
        '''
        if len(point_array) < 4:
            return point_array

        need_again = True
        round = 1
        # 一轮可能无法全部去除，需要多轮
        while need_again and len(point_array) > 3 and round < len(point_array):
            need_again = False

            i = 0
            while i < len(point_array)-1:  # 排序后0点一定是凸点，不用判断
                x1 = point_array[(i+1)][0]-point_array[i][0]
                y1 = point_array[(i+1)][1]-point_array[i][1]
                x2 = point_array[(i+2) % len(point_array)
                                 ][0]-point_array[i+1][0]
                y2 = point_array[(i+2) % len(point_array)
                                 ][1]-point_array[i+1][1]
                PxQ = self.__cross_product(x1, y1, x2, y2)

                if PxQ >= 0:  # 移除点
                    del point_array[i+1]

                    # logger.info("round {},del idx={}", round, i)

                    need_again = True
                    continue
                i += 1

            round += 1

        return point_array

        # 找出相交的点，和在对方内部的点，排序，计算面积

    def is_point_in_polygon(self, point: list, point_array: list) -> bool:
        '''
        点是否在多边形内部
        1,要先排序 
        2,必须是凸多边形       
        '''
        if len(point_array) < 3:
            return False

        for i in range(len(point_array)):
            x1 = point_array[i][0]-point[0]
            y1 = point_array[i][1]-point[1]
            x2 = point_array[(i+1) % len(point_array)][0]-point[0]
            y2 = point_array[(i+1) % len(point_array)][1]-point[1]
            PxQ = self.__cross_product(x1, y1, x2, y2)

            # logger.debug("i= {} pxq={}",i,PxQ)

            if PxQ > 0:  # ==0时候表示在某一条边上，视为内部
                return False

        return True

    def is_line_across(self, ax, ay, bx, by, cx, cy, dx, dy):
        '''
        返回ab和cd两线是否相交：有一个点在另一条线上情况，定义为不相交，该点由点在多边形内部处理
        '''

        x_ac = cx-ax
        y_ac = cy-ay

        x_ad = dx-ax
        y_ad = dy-ay

        x_ab = bx-ax
        y_ab = by-ay

        ACxAB = self.__cross_product(x_ac, y_ac, x_ab, y_ab)

        ADxAB = self.__cross_product(x_ad, y_ad, x_ab, y_ab)

        if ACxAB*ADxAB >= 0:  # 方向相反，不交叉
            return False

        x_ca = ax-cx
        y_ca = ay-cy

        x_cd = dx-cx
        y_cd = dy-cy

        x_cb = bx-cx
        y_cb = by-cy

        CAxCD = self.__cross_product(x_ca, y_ca, x_cd, y_cd)

        CBxCD = self.__cross_product(x_cb, y_cb, x_cd, y_cd)

        if CAxCD*CBxCD >= 0:  # 方向相反，不交叉
            return False

        return True

    def cross_point(self, ax, ay, bx, by, cx, cy, dx, dy) -> list:
        '''
        返回ab和cd两线交点,返回格式[Ox,Oy]
        1，要先判断是否相交,出错返回None
        '''

        x_ac = cx-ax
        y_ac = cy-ay

        x_ad = dx-ax
        y_ad = dy-ay

        x_ab = bx-ax
        y_ab = by-ay

        x_cd = dx-cx
        y_cd = dy-cy

        ACxAD = self.__cross_product(x_ac, y_ac, x_ad, y_ad)

        ABxCD = self.__cross_product(x_ab, y_ab, x_cd, y_cd)

        assert 0 != ABxCD, "要先判断是否相交"

        rate = abs(ACxAD/ABxCD)

        ox = ax+rate*(bx-ax)

        oy = ay+rate*(by-ay)

        return [ox, oy]

    def polygon_area(self, point_array: list) -> float:
        '''
        计算凸多边形面积
        要先排序
        '''

        if len(point_array) < 3:
            return 0.0

        area = 0.0

        for i in range(len(point_array)-2):
            x1 = point_array[i+1][0]-point_array[0][0]
            y1 = point_array[i+1][1]-point_array[0][1]
            x2 = point_array[i+2][0]-point_array[0][0]
            y2 = point_array[i+2][1]-point_array[0][1]

            PxQ = self.__cross_product(x1, y1, x2, y2)

            area += 0.5*abs(PxQ)

        return area

    def two_polygon_cross_area(self, point_array: list, point_array2: list) -> list:
        '''
        计算凸多边形交叉面积        
        数据格式为[[x1,y1],[x2,y2].......]
        1，要先排序
        2，要先转成凸多边型

        方法是，求所有在对方内部的点和相交边的焦点，组成的多变型即交叉面积
        '''
        pList = []

        if len(point_array) < 3 or len(point_array2) < 3:
            return pList

        # 在对方内部的点
        for i, p in enumerate(point_array2):
            if self.is_point_in_polygon(p, point_array):
                # logger.debug("number {} point={} in p1",i+1,p)
                pList.append(p)

        for i, p in enumerate(point_array):
            if self.is_point_in_polygon(p, point_array2):
                # logger.debug("number {} point={} in p2",i+1,p)
                pList.append(p)

        # 交叉点
        for i in range(len(point_array)):
            ax = point_array[i][0]
            ay = point_array[i][1]
            bx = point_array[(i+1) % len(point_array)][0]
            by = point_array[(i+1) % len(point_array)][1]

            for k in range(len(point_array2)):
                cx = point_array2[k][0]
                cy = point_array2[k][1]
                dx = point_array2[(k+1) % len(point_array2)][0]
                dy = point_array2[(k+1) % len(point_array2)][1]

                if self.is_line_across(ax, ay, bx, by, cx, cy, dx, dy):  # 相交，求交点
                    pList.append(self.cross_point(
                        ax, ay, bx, by, cx, cy, dx, dy))

        # 重新排序
        pList = self.sort_points(pList)

        return pList

    def iou(self, point_array: list, point_array2: list) -> float:
        '''        
        @param [[x1,y1],[x2,y2].....],[[x1,y1],[x2,y2].....]
        @return IOU in float
        '''
        if point_array is None or point_array2 is None or len(point_array) < 3 or len(point_array2) < 3:
            logger.warning("不是多边型")
            return 0.0

        point_array = self.sort_points(point_array)  # 排序
        point_array = self.convex_to_converx(point_array)  # 转换成凸多边形

        point_array2 = self.sort_points(point_array2)
        point_array2 = self.convex_to_converx(point_array2)

        # 计算面积
        area1 = self.polygon_area(point_array)
        area2 = self.polygon_area(point_array2)

        cross_point = self.two_polygon_cross_area(point_array, point_array2)
        cross_area = self.polygon_area(cross_point)

        iou = cross_area/(area1+area2-cross_area)

        # logger.debug("A1={},A2={},A_cross={},iou={}",
        #              area1, area2, cross_area, iou)

        return iou

    def center_point(self, point_array: list):
        '''
        求重心 
        @param [[x1,y1],[x2,y2].....]
        @return [x,y]
        '''
        if len(point_array) < 2:
            return point_array
        c_x = 0.0
        c_y = 0.0
        for _, value in enumerate(point_array):
            c_x += value[0]
            c_y += value[1]
        return [c_x/len(point_array), c_y/len(point_array)]

    def rotate(self, point_array, center_point, angle,clockwise=True) -> list:
        '''
        旋转 
        @param [[x1,y1],[x2,y2].....],[xo,yo],pi,True
        @return [[x1,y1],[x2,y2].....]
        '''
        x_c = center_point[0]
        y_c = center_point[1]

        if clockwise:
            angle=-angle

        sin_a = math.sin(angle)
        cos_a = math.cos(angle)

        m = np.array([[cos_a, -sin_a, x_c-x_c*cos_a+y_c*sin_a],
                      [sin_a, cos_a, y_c-x_c*sin_a-y_c*cos_a], [0, 0, 1]])

        # print("m=")
        # print(m)

        result = []

        for k, p in enumerate(point_array):

            m2 = np.array([[p[0]], [p[1]], [1]])
            # print("m2=",m2)

            r = np.matmul(m, m2)

            # print("r=",r)

            result.append([r[0][0], r[1][0]])

        # print(result)

        return result
    
    
